罗德里格斯公式的推导 - 一般路过鼠鼠的初始台地

将旋转向量到旋转矩阵进行转换的过程由罗德里格斯公式（Rodrigues’s Formula）表明：

R = \cos\theta I + (1 - \cos\theta) nn^T + \sin\theta n^\wedge

n^{\wedge}

为**反对称矩阵【Skew-symmetric Matrix】**，反对称矩阵满足

n^T = -n

，反之亦然：

a^{\wedge} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}

向量的反对称矩阵？巧了，我也不知道，继续看吧。假设有向量 $v = (a, b, c)$ ，以及 $x = (x, y, z)$ ：

v \times x = \begin{vmatrix} i & j & k \\ a & b & c \\ x & y & z \end{vmatrix} \\ = (bz-cy)i+(cx-az)j+(ay-bx)k \\ = \begin{bmatrix} 0 & -c & b \\ c & 0 & -a \\ -b & a & 0 \end{bmatrix} \times \begin{bmatrix} x \\ y \\ z \end{bmatrix} = v^{\wedge} x

反对称矩阵将二个定义在同一个坐标系的向量叉乘运算转换为矩阵和向量的乘法运算。 —— CSDN《三维几何学基础：向量、点乘、叉乘、反对称矩阵》

推导过程：

如图所示，我们需要把向量 $v$ 旋转到 $v_{rot}$ 的位置

$v_{||}$ 如果被旋转向量 $v$ 与旋转轴 $k$ （ $***k$ * 为单位向量**）相互垂直，那旋转变换不难表示。而对于与旋转轴 $k$ 呈任意角度的向量 $v$ ，可以通过正交分解，把被旋转向量转化为与旋转轴平行的分量 $v_{||}$ 和与旋转轴垂直的分量 $v_{\bot}$ ，其中与旋转轴平行的分量在旋转中是不变的，而与旋转轴垂直的分量 $v_{\bot}$ 则恰好旋转了角度 $\theta$ ，把与旋转轴平行的分量与旋转以后的与旋转轴垂直的分量加在一起，即可得到旋转以后的向量。 ——[百度百科](https://baike.baidu.com/item/%E7%BD%97%E5%BE%B7%E9%87%8C%E6%A0%BC%E6%97%8B%E8%BD%AC%E5%85%AC%E5%BC%8F/18878562)
对 $v$ 做正交分解得到：

v = v_{||} + v_{\bot}

通过向量投影，得到 $V_{||}$ ：

v_{||} = \left(v{\cdot}k\right)k

通过向量减法：

v_{\bot} = v - v_{||} = v - (v{\cdot}k)k

利用外积得到与 $v_{\bot}$ 和 $k$ 都垂直的向量 $w$ ：

w = k \times v_{\bot} = k \times (v - (v{\cdot}k)k) = k \times v

旋转后向量为：

v_{\bot{rot}} = \cos{\theta}v_{\bot} + \sin{\theta}w =\cos\theta (v-(v\cdot k)k) + \sin\theta k \times v

与 $v_{||}$ 相加即可得到旋转后的向量表达式：

v_{rot} = v_{||} + v_{\bot rot} = \cos\theta v + (1 - \cos\theta)(v \cdot k)k + \sin\theta k \times v

然后我们得到了向量表达式，通过将 $k$ 和 $v$ 写为列向量：

k = \begin{pmatrix} k_x \\ k_y \\ k_z \end{pmatrix} , v = \begin{pmatrix} v_x \\ v_y \\ v_z \end{pmatrix}

旋转后的向量则可以表示为：

v_{rot} = Rv

R = \cos\theta I + (1 - \cos\theta) \begin{pmatrix} k_x \\ k_y \\ k_z \end{pmatrix} \begin{pmatrix} k_x,k_y,k_z \end{pmatrix} + \sin\theta \begin{pmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{pmatrix}

即罗德里格斯公式：

R = \cos\theta I + (1 - \cos\theta)kk^T+\sin\theta k^{\wedge}